
import time
import wx
import vtk
import math
import numpy
try:
    import extended.BaseCalc as BaseCalc
except ImportError:
    import BaseCalc


def create(parent):    
    return CoordinateSystems(parent)
    
class CoordinateSystems:
        
    def __init__(self, parent):
        print "CoordinateSystems module loaded."        
        self.parent = parent
        self.baseCalc = BaseCalc.create(self, self)

    def DefineXYZThorax(self, IJ, C7, midIJC7, midPXT8):
        #Ot: The origin coincident with IJ
        coordThorax_O = IJ
        #Yt: The line connecting the midpoint between PX and T8 
        # and the midpoint between IJ and C7, pointing upward.
        coordThorax_Y = self.baseCalc.SubtractPoints(midIJC7, midPXT8)
        coordThorax_Y = self.baseCalc.Normalize(coordThorax_Y)
        #Zt: The line perpendicular to the plane formed by
        #IJ, C7, and the midpoint between PX and T8, pointing to the Right.
        TempA = self.baseCalc.SubtractPoints(IJ, midPXT8)
        TempB = self.baseCalc.SubtractPoints(IJ, C7)
        TempA = self.baseCalc.Normalize(TempA)        
        TempB = self.baseCalc.Normalize(TempB)        
        coordThorax_Z = [0,0,0]
        vtk.vtkMath.Cross(TempB, TempA, coordThorax_Z)
        coordThorax_Z = self.baseCalc.Normalize(coordThorax_Z)
        #Xt: The common line perpendicular to the Zt- and
        #Yt-axis, pointing forwards.
        coordThorax_X = [0,0,0]
        vtk.vtkMath.Cross(coordThorax_Y, coordThorax_Z, coordThorax_X)
        coordThorax_X = self.baseCalc.Normalize(coordThorax_X)
        self.OXYZThorax = [coordThorax_O, coordThorax_X, coordThorax_Y, coordThorax_Z]
        return self.OXYZThorax


    def DefineXYZClavicle(self, side):
        #The origin coincident with SC
        coordClavicle_O = self.GetCoords("Right_SC")
        #Zc: The line connecting SC and AC, pointing to AC
        coordClavicle_Z = self._root.baseCalc.SubtractPoints(self.GetCoords("Right_AC"), self.GetCoords("Right_SC"))
        coordClavicle_Z = self._root.baseCalc.Normalize(coordClavicle_Z)
        #Xc: The line perpendicular to Zc and Yt, pointing forward
        #Note that the Xc-axis is defined with
        #respect to the vertical axis of the thorax (Ytaxis)
        #because only two bonylandmarks can be discerned at the clavicle.
        TempA = self._root.baseCalc.SubtractPoints(self.midIJC7, self.midPXT8)
        TempA = self._root.baseCalc.Normalize(TempA) #(Yt)       
        coordClavicle_X = [0,0,0]
        vtk.vtkMath.Cross(TempA, coordClavicle_Z, coordClavicle_X)
        coordClavicle_X = self._root.baseCalc.Normalize(coordClavicle_X)
        #Yc: The common line perpendicular to the Xc- and Zc-axis, pointing upward.
        coordClavicle_Y = [0,0,0]
        vtk.vtkMath.Cross(coordClavicle_Z, coordClavicle_X, coordClavicle_Y)
        coordClavicle_Y = self._root.baseCalc.Normalize(coordClavicle_Y)
        self.Right_OXYZClavicle = [coordClavicle_O, coordClavicle_X, coordClavicle_Y, coordClavicle_Z]
        
    def DefineXYZScapula(self, side):
        #The origin coincident with AA.
        coordScapula_O = self.GetCoords("Right_AA")
        #Zs: The line connecting TS and AA, pointing to AA.       
        coordScapula_Z = self._root.baseCalc.SubtractPoints(self.GetCoords("Right_AA"), self.GetCoords("Right_TS"))
        #coordScapula_Z = self._root.baseCalc.Normalize(coordScapula_Z)
        vtk.vtkMath.Normalize(coordScapula_Z)

        #Xs: The line perpendicular to the plane formed by AI, AA, and TS, pointing forward. 
        TempA = self._root.baseCalc.SubtractPoints(self.GetCoords("Right_AI"), self.GetCoords("Right_TS"))
        TempB = self._root.baseCalc.SubtractPoints(self.GetCoords("Right_AI"), self.GetCoords("Right_AA"))
        TempA = self._root.baseCalc.Normalize(TempA)        
        TempB = self._root.baseCalc.Normalize(TempB)        
        coordScapula_X = [0,0,0]
        vtk.vtkMath.Cross(TempA, TempB, coordScapula_X)
        coordScapula_X = self._root.baseCalc.Normalize(coordScapula_X)

        #Ys: The common line perpendicular to the Xs- and Zs-axis, pointing upward.
        coordScapula_Y = [0,0,0]
        vtk.vtkMath.Cross(coordScapula_Z, coordScapula_X, coordScapula_Y)
        coordScapula_Y = self._root.baseCalc.Normalize(coordScapula_Y)
        
        #~ #Faulty FOB coords...:
        #~ tmp = coordScapula_Z
        #~ coordScapula_Z = self._root.baseCalc.MultiplyVector(coordScapula_X, -1)
        #~ coordScapula_X = tmp
        
        self.Right_OXYZScapula = [coordScapula_O, coordScapula_X, coordScapula_Y, coordScapula_Z]
            

    def DefineXYZHumerus(self, side):
        coordHumerus_O = self.GetCoords("Right_GH")
        #Yh1: The line connecting GH and the midpoint of EL and EM, pointing to GH.
        coordHumerus_Y = self._root.baseCalc.SubtractPoints(self.GetCoords("Right_GH"), self.Right_midELEM)
        coordHumerus_Y = self._root.baseCalc.Normalize(coordHumerus_Y)

        #Xh1: The line perpendicular to the plane formed by EL, EM, and GH, pointing forward.
        coordHumerus_Ya = self._root.baseCalc.SubtractPoints(self.GetCoords("Right_GH"), self.GetCoords("Right_EL"))
        coordHumerus_Yb = self._root.baseCalc.SubtractPoints(self.GetCoords("Right_GH"), self.GetCoords("Right_EM"))
        coordHumerus_Ya = self._root.baseCalc.Normalize(coordHumerus_Ya)
        coordHumerus_Yb = self._root.baseCalc.Normalize(coordHumerus_Yb)
        coordHumerus_X = [0,0,0]
        vtk.vtkMath.Cross(coordHumerus_Yb, coordHumerus_Ya, coordHumerus_X)
        coordHumerus_X = self._root.baseCalc.Normalize(coordHumerus_X)

        #Zh1: The common line perpendicular to the Yh1- and Xh1-axis, pointing to the Right.
        coordHumerus_Z = [0,0,0]
        vtk.vtkMath.Cross(coordHumerus_X, coordHumerus_Y, coordHumerus_Z)
        coordHumerus_Z = self._root.baseCalc.Normalize(coordHumerus_Z)
        
        #Faulty FOB coords...:
        tmp = coordHumerus_Z
        coordHumerus_Z = self._root.baseCalc.MultiplyVector(coordHumerus_X, -1)
        coordHumerus_X = tmp

        self.Right_OXYZHumerus = [coordHumerus_O, coordHumerus_X, coordHumerus_Y, coordHumerus_Z]
        
        
    def DefineXYZForearm(self, side):
        coordForearm_O = self.GetCoords("Right_US")
        #Yf : The line connecting US and the midpoint
        #between EL and EM, pointing proximally.
        coordForearm_Y = self._root.baseCalc.SubtractPoints(self.Right_midELEM, self.GetCoords("Right_US"))
        coordForearm_Y = self._root.baseCalc.Normalize(coordForearm_Y)

        #Xf : The line perpendicular to the plane through US,
        #RS, and the midpoint between EL and EM, pointing forward.
        TempA = self._root.baseCalc.SubtractPoints(self.Right_midELEM, self.GetCoords("Right_US"))
        TempB = self._root.baseCalc.SubtractPoints(self.Right_midELEM, self.GetCoords("Right_RS"))   
        TempA = self._root.baseCalc.Normalize(TempA)        
        TempB = self._root.baseCalc.Normalize(TempB)        
        coordForearm_X = [0,0,0]
        vtk.vtkMath.Cross(TempB, TempA, coordForearm_X)
        coordForearm_X = self._root.baseCalc.Normalize(coordForearm_X)
        #*Hoe zorgen dat coordForearm_X naar voren wijst??
        
        #Zf : The common line perpendicular to the Xf and
        #Yf -axis, pointing to the Right.
        coordForearm_Z = [0,0,0]
        vtk.vtkMath.Cross(coordForearm_X, coordForearm_Y, coordForearm_Z)
        coordForearm_Z = self._root.baseCalc.Normalize(coordForearm_Z)

        self.Right_OXYZForearm = [coordForearm_O, coordForearm_X, coordForearm_Y, coordForearm_Z]
    
    
    
    def MatchCoordinateSystemAtoB(self, oxyzFirst, oxyzSecond):
        #returns a transform that, when applied, transforms coordinate system xyzFirst
        #to xyzSecond, i.e. x=x, y=y, z=z.
        total_lmtransf = vtk.vtkLandmarkTransform()
        assen1 = vtk.vtkPoints()
        assen1.SetNumberOfPoints(4)
        assen1.SetPoint(0, oxyzFirst[0])
        assen1.SetPoint(1, oxyzFirst[1])
        assen1.SetPoint(2, oxyzFirst[2])
        assen1.SetPoint(3, oxyzFirst[3])
        
        assen2 = vtk.vtkPoints()
        assen2.SetNumberOfPoints(4)
        assen2.SetPoint(0, oxyzSecond[0])
        assen2.SetPoint(1, oxyzSecond[1])
        assen2.SetPoint(2, oxyzSecond[2])
        assen2.SetPoint(3, oxyzSecond[3])
        total_lmtransf.SetSourceLandmarks(assen1)
        total_lmtransf.SetTargetLandmarks(assen2)
        total_lmtransf.Update()
        
        total_matrix1 = total_lmtransf.GetMatrix()
        total_newtransf = vtk.vtkTransform()
        total_newtransf.SetMatrix(total_matrix1)
        return total_newtransf
    